/**
 * 霍尔传感器探测 四轮TIM定义在bsp_def.h
 *
 */

#include "bsp_wheel_sensor.h"

#include <stdio.h>

#include "FreeRTOS.h"
#include "ax_oled_1305.h"
#include "bsp_def.h"
#include "cmsis_os.h"
#include "tim.h"

// 轮子配置数组
typedef struct {
    TIM_HandleTypeDef* timer;
    int8_t direction;
} WheelConfig_t;

// 四轮配置配置描述
static const WheelConfig_t wheel_configs[] = {
    {BSP_LF_DETECT_TIM, 1},   // 左前轮
    {BSP_RF_DETECT_TIM, -1},  // 右前轮
    {BSP_LB_DETECT_TIM, 1},   // 左后轮
    {BSP_RB_DETECT_TIM, -1},  // 右后轮
};

BSP_Wheel_Sensor_HandleTypeDef hws;

void BSP_Wheel_Detect_Init(void) {
    // 电机编码器读取初始化
    HAL_TIM_Encoder_Start(BSP_LF_DETECT_TIM, BSP_LF_DETECT_TIM_CHANNEL);  // 电机A
    HAL_TIM_Encoder_Start(BSP_RF_DETECT_TIM, BSP_RF_DETECT_TIM_CHANNEL);  // 电机B
    HAL_TIM_Encoder_Start(BSP_LB_DETECT_TIM, BSP_LB_DETECT_TIM_CHANNEL);  // 电机C
    HAL_TIM_Encoder_Start(BSP_RB_DETECT_TIM, BSP_RB_DETECT_TIM_CHANNEL);  // 电机D

    // 初始化编码器计数器
    for (int i = 0; i < WHEEL_COUNT; i++) {
        __HAL_TIM_SET_COUNTER(wheel_configs[i].timer, TIM_LOOP_MARK);
        hws.wheelSpeeds[i].tick_time = HAL_GetTick();
    }
    printf("Motor Encoder Init OK!\r\n");
}

// 循环读取麦轮速度
void BSP_Wheel_Detect_Loop(void) {
    // 处理所有轮子当前速度存入count
    for (int i = 0; i < WHEEL_COUNT; i++) {
        // 计算当前速度
        int32_t current_count = __HAL_TIM_GET_COUNTER(wheel_configs[i].timer) - TIM_LOOP_MARK;  // 取得当前计数器--TIM_LOOP_MARK 取得当前计数值
        int32_t times = (HAL_GetTick() - hws.wheelSpeeds[i].tick_time);

#if DEBUG_MOTOR_DETECT
        printf("times:%ld, current_count:%ld\n", times, current_count);
#endif
        // 速度计算公式：每分钟圈速度 = 当前计数* 1000（转秒）/（现在时间/ms-过去时间/ms）* 60 /每圈计数器 * 方向符号
        hws.wheelSpeeds[i].speed = (current_count * 1000 / times * 60 / ENCODER_COUNTS_PER_REV * wheel_configs[i].direction);
        // printf("debug------------------------------: %ld, %ld, %ld, %ld\r\n", HAL_GetTick(), current_count, (current_count - TIM_LOOP_MARK) * 1000 / times , wheel_speeds[i].speed);
        // 下次计算使用
        hws.wheelSpeeds[i].tick_time = HAL_GetTick();
        // 重置计数器
        __HAL_TIM_SET_COUNTER(wheel_configs[i].timer, TIM_LOOP_MARK);
    }
    char display_text[32] = {0};
    sprintf(display_text, "LF:%4ld,RF:%4ld", hws.wheelSpeeds[0].speed, hws.wheelSpeeds[1].speed);
    AX_OLED_DispStr(0, 5, (uint8_t*)display_text, 0);  // OLED输出速度
    sprintf(display_text, "LB:%4ld,RB:%4ld", hws.wheelSpeeds[2].speed, hws.wheelSpeeds[3].speed);
    AX_OLED_DispStr(0, 6, (uint8_t*)display_text, 0);  // OLED输出速度

#if DEBUG_MOTOR_DETECT
    // 使用wheel_speeds数组中的速度值
    printf("Wheel Speed < 300)= LF:%ld, RF:%ld, LB:%ld, RB:%ld\r\n",
           hws.wheelSpeeds[0].speed, hws.wheelSpeeds[1].speed, hws.wheelSpeeds[2].speed, hws.wheelSpeeds[3].speed);
#endif
}
